#include <actionlib/server/simple_action_server.h>
#include <actionlib_test/DoDishesAction.h>

// 运行方法：
// roscore
// rosrun actionlib_test action_server_dish
// rosrun actionlib_test action_client_dish

typedef actionlib::SimpleActionServer<actionlib_test::DoDishesAction> Server;
static void execute(const actionlib_test::DoDishesGoalConstPtr &goal, Server *as){
	actionlib_test::DoDishesFeedback feedback;
	actionlib_test::DoDishesResult result;
	printf("dishwasher_id=%u\n", goal->dishwasher_id);
	
	for (int i = 1; i <= 10; i++){
		if (as->isPreemptRequested()){
			printf("cancelled!\n");
			result.total_dishes_cleaned = 30000;
			as->setPreempted(result);
			return;
		}
		printf("step %d\n", i);
		feedback.percent_complete = i * 10;
		as->publishFeedback(feedback);
		sleep(1);
	}
	
	printf("ok\n");
	result.total_dishes_cleaned = 1000;
	as->setSucceeded(result);
}
 
int main(int argc, char** argv){
	ros::init(argc, argv, "do_dishes_server");
	ros::NodeHandle n;
	Server server(n, "do_dishes", boost::bind(execute, _1, &server), false);
	server.start();
	ros::spin();
	return 0;
}